/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  Collision detector
 */

#ifndef COLLISION_DETECTOR_H_
#define COLLISION_DETECTOR_H_

#include "ros/ros.h"
#include <tf/tf.h>
#include <tf/transform_listener.h>

#define Pii 3.1415926536
#define TO_RADIANS(a) ((a)/57.295779)

#define s1 (sin(TO_RADIANS(35)))
#define c1 (cos(TO_RADIANS(35)))
#define s2 (sin(TO_RADIANS(2)))
#define c2 (cos(TO_RADIANS(2)))
#define s3 (sin(TO_RADIANS(90)))
#define c3 (cos(TO_RADIANS(90)))
#define s4 (sin(TO_RADIANS(90)))
#define c4 (cos(TO_RADIANS(90)))

#define s1n (-sin(TO_RADIANS(35)))
#define c1n (-cos(TO_RADIANS(35)))
#define s2n (-sin(TO_RADIANS(2)))
#define c2n (-cos(TO_RADIANS(2)))
#define s3n (-sin(TO_RADIANS(90)))
#define c3n (-cos(TO_RADIANS(90)))
#define s4n (-sin(TO_RADIANS(90)))
#define c4n (-cos(TO_RADIANS(90)))

typedef struct {
    double x;
    double y;
    double z;
} Pos;

class CoordinatorConverter
{
public:
    CoordinatorConverter();
    ~CoordinatorConverter(){}

    void saveAngles(const geometry_msgs::Vector3::ConstPtr& vector);
    void saveRobotPosition(const std_msgs::Bool::ConstPtr& msg);
    bool convertCoordinates(group6::convertCoordinates::Request &req, group6::convertCoordinates::Response &res);
    bool requestPose();
    Pos convert(Pos position, int kinectElevation = 0, int kinectYaw = 0);
    
    Pos getServoAngles() { return mServoAngles; }
    double limitAngle(double angle);

private:
    // ROS handles
    ros::NodeHandle         mNode;
    ros::Subscriber         mServoAnglesSubscriber;
    ros::ServiceServer      mConvertCoordinatesService;
    tf::TransformListener   mListener;
    tf::StampedTransform    mCurrentTransform;

    // Robot parameters
    Pos     mServoAngles;
    Pos     mBasePosition;
    double  mHeading;
    bool    mRealtime;
};

#endif
